#include <stdio.h>
#include "unistd.h"
#include "ultrasound.h"
#include "rpi.h"




#define PIN_TRIG RPI_GPIO_P1_16



int main(int argc, char **argv)
{
  RPi::initialize();

  uint8_t pins[6];
  pins[0] = RPI_GPIO_P1_18;
  pins[1] = RPI_GPIO_P1_19;
  pins[2] = RPI_GPIO_P1_21;
  pins[3] = RPI_GPIO_P1_22;
  //pins[4] = RPI_GPIO_P1_23;
  pins[4] = RPI_GPIO_P1_24;

  int ret;
  int timeout = 25;
  int minrest = 200000;

  RPi::setupOutput(PIN_TRIG);
  int state[5];
  for (int i = 0; i < 5; i++)
  {
    RPi::setupInput(pins[i]);
    //RPi::setupInterrupt(pins[i], RPi::Both);
  }
  while (true)
  {
    long m_trigger_time = RPi::usec();
    RPi::writeOutput(PIN_TRIG, RPi::High);
    usleep(100);
    //usleep(3000000);
    RPi::writeOutput(PIN_TRIG, RPi::Low);

    for (int i = 0; i < 5; i++)
      state[i]=0;
    for (int t = 0; t < 200; t++)
    {
      for (int i = 0; i < 5; i++)
      {
        if (RPi::readInput(pins[i]) == RPi::High)
          state[i]++;
      }
      usleep(50);
    }
    for (int i = 0; i < 5; i++)
    {
      printf("%6d ", state[i]);
    }
    printf("\n");
    /*
    if (RPi::readInput(pins[0]) == RPi::High)
      printf("high\n");
    else
      printf("low\n");
    for (int l = 0; l < 2; l++)
    {
      ret = RPi::pollInterrupt(pins[0], timeout);
      if (ret == -1) 
      {
        printf("poll error in ultrasound device \n");
        break;
      }
      if (ret ==  -2) 
      {
        printf("timeout\n");
        continue;
      }

      if (ret == 1)
      {
        printf("sent\n");
      }
      if (ret == 0)
      {
        printf("ret\n");
      }
    }
    */
    long diff = RPi::usec() - m_trigger_time;
    if (diff > 0 && diff < minrest)
      usleep(minrest-diff);



  }

//  pins[0] = RPI_GPIO_P1_21;

  
  Ultrasound u(PIN_TRIG, pins, 1);
  u.start();

  while(true)
    usleep(20000);
  return 0;
}

